d1abd5502b215f56ec5ffcc21a8386e8fba4c588,Atlas/src/us/ihmc/atlas/StepAdjustmentVisualizers/StepAdjustmentExampleGraphic.java,StepAdjustmentExampleGraphic,initialize,#,259
Before Change
icpOptimizationController.setStepDurations(doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
icpOptimizationController.addFootstepToPlan(nextFootstep);
icpOptimizationController.addFootstepToPlan(nextNextFootstep);
icpOptimizationController.addFootstepToPlan(nextNextNextFootstep);
RobotSide supportSide = nextFootstep.getRobotSide().getOppositeSide();
icpPlanner.setSupportLeg(supportSide);
icpPlanner.initializeForTransfer(yoTime.getDoubleValue());
icpPlanner.getDesiredCapturePointPositionAndVelocity(desiredICP, desiredICPVelocity, yoTime.getDoubleValue() + doubleSupportDuration.getDoubleValue());
icpOptimizationController.initializeForTransfer(yoTime.getDoubleValue(), supportSide, omega0.getDoubleValue());
icpOptimizationController.compute(yoTime.getDoubleValue() + doubleSupportDuration.getDoubleValue(), desiredICP, desiredICPVelocity, desiredICP, omega0.getDoubleValue());
/** do single support **/
FootSpoof footSpoof = contactableFeet.get(supportSide.getOppositeSide());
FramePose nextSupportPose = footPosesAtTouchdown.get(supportSide.getOppositeSide());
nextSupportPose.setToZero(nextFootstep.getSoleReferenceFrame());
nextSupportPose.changeFrame(ReferenceFrame.getWorldFrame());
footSpoof.setSoleFrame(nextSupportPose);
contactStates.get(supportSide.getOppositeSide()).clear();
if (nextFootstep.getPredictedContactPoints() == null)
contactStates.get(supportSide.getOppositeSide()).setContactFramePoints(footSpoof.getContactPoints2d());
else
contactStates.get(supportSide.getOppositeSide()).setContactPoints(nextFootstep.getPredictedContactPoints());
bipedSupportPolygons.updateUsingContactStates(contactStates);
icpPlanner.clearPlan();
icpOptimizationController.clearPlan();
timing.setTimings(singleSupportDuration.getDoubleValue(), doubleSupportDuration.getDoubleValue());
icpPlanner.addFootstepToPlan(nextFootstep, timing);
icpPlanner.addFootstepToPlan(nextNextFootstep, timing);
icpPlanner.addFootstepToPlan(nextNextNextFootstep, timing);
icpOptimizationController.setStepDurations(doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
icpOptimizationController.addFootstepToPlan(nextFootstep);
icpOptimizationController.addFootstepToPlan(nextNextFootstep);
icpOptimizationController.addFootstepToPlan(nextNextNextFootstep);
icpPlanner.setSupportLeg(supportSide);
icpPlanner.initializeForSingleSupport(yoTime.getDoubleValue());
After Change
icpOptimizationController.clearPlan();
icpOptimizationController.setStepDurations(doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(0));
icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(1));
icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(2));
RobotSide supportSide = plannedFootsteps.get(0).getRobotSide().getOppositeSide();
icpPlanner.setSupportLeg(supportSide);
icpPlanner.initializeForTransfer(yoTime.getDoubleValue());
icpPlanner.getDesiredCapturePointPositionAndVelocity(desiredICP, desiredICPVelocity, yoTime.getDoubleValue() + doubleSupportDuration.getDoubleValue());
icpOptimizationController.initializeForTransfer(yoTime.getDoubleValue(), supportSide, omega0.getDoubleValue());
icpOptimizationController.compute(yoTime.getDoubleValue() + doubleSupportDuration.getDoubleValue(), desiredICP, desiredICPVelocity, desiredICP, omega0.getDoubleValue());
/** do single support **/
FootSpoof footSpoof = contactableFeet.get(supportSide.getOppositeSide());
FramePose nextSupportPose = footPosesAtTouchdown.get(supportSide.getOppositeSide());
nextSupportPose.setToZero(plannedFootsteps.get(0).getSoleReferenceFrame());
nextSupportPose.changeFrame(ReferenceFrame.getWorldFrame());
footSpoof.setSoleFrame(nextSupportPose);
contactStates.get(supportSide.getOppositeSide()).clear();
if (plannedFootsteps.get(0).getPredictedContactPoints() == null)
contactStates.get(supportSide.getOppositeSide()).setContactFramePoints(footSpoof.getContactPoints2d());
else
contactStates.get(supportSide.getOppositeSide()).setContactPoints(plannedFootsteps.get(0).getPredictedContactPoints());
bipedSupportPolygons.updateUsingContactStates(contactStates);
icpPlanner.clearPlan();
timing.setTimings(singleSupportDuration.getDoubleValue(), doubleSupportDuration.getDoubleValue());
icpPlanner.addFootstepToPlan(plannedFootsteps.get(0), timing);
icpPlanner.addFootstepToPlan(plannedFootsteps.get(1), timing);
icpPlanner.addFootstepToPlan(plannedFootsteps.get(2), timing);
icpOptimizationController.clearPlan();
icpOptimizationController.setStepDurations(doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(0));
icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(1));
icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(2));